#include "AlgoEvaluatorModule0Topography.h"
#include <cfloat> // For DBL_MAX
#include <algorithm> // For std::min

class algo::AlgoEvaluatorModule0TopographyPrivate
{
public:

};

algo::AlgoEvaluatorModule0Topography::AlgoEvaluatorModule0Topography()
	: d(new AlgoEvaluatorModule0TopographyPrivate())
{

}

algo::AlgoEvaluatorModule0Topography::~AlgoEvaluatorModule0Topography()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule0Topography::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>({ algobase::EEDT_Depth });
}

int algo::AlgoEvaluatorModule0Topography::exec(
	const algobase::Path& path,
	double& result,
	algobase::EvaluateModuleCoords& resultCoords)
{
	if (path.empty())
	{
		result = 0.0; // Or a value indicating no path
		resultCoords.clear();
		return 0;
	}

	auto db = getDB();
	if (!db) {
		// Database not available, cannot perform evaluation.
		return -1;
	}

	double min_clearance = DBL_MAX;
	resultCoords.resize(path.size());

	for (size_t i = 0; i < path.size(); ++i)
	{
		resultCoords[i].Assign(path[i]);

		// Assuming mCoordZ[2] is altitude (higher is shallower) and terrain_depth is also altitude.
		// A positive clearance means the vehicle is above the seabed.
		double vehicle_altitude = path[i].mCoordZ[2];
		double terrain_altitude = db->getData(algobase::EEDT_Depth, getTaskConfig().mStartTime, path[i].mCoordZ, 9999.0f);
		
		double current_clearance;
		if (terrain_altitude == 9999.0f) {
			// If terrain data is unavailable, return a neutral but not optimal clearance.
			// This avoids harsh penalties while still being less desirable than known safe areas.
			current_clearance = 150.0; // Corresponds to the sigmoid midpoint (score 0.5)
		} else {
			current_clearance = vehicle_altitude - terrain_altitude;
		}

		resultCoords[i].mRawValue = current_clearance;
		resultCoords[i].mResult = calcScore(current_clearance); // Store individual point clearance for potential debug/analysis.

		if (current_clearance < min_clearance) {
			min_clearance = current_clearance;
		}
	}

	// The final result for this module is the minimum clearance found along the entire path.
	// This value will be the "Raw Value" for the EL2IT_0Topography index.
	result = (min_clearance == DBL_MAX) ? 0.0 : calcScore(min_clearance);

	return 0;
}

double algo::AlgoEvaluatorModule0Topography::calcScore(double rawValue) const
{
	return benefitSigmoid(rawValue, 150.0, 50.0) * getWeight();
}

double algo::AlgoEvaluatorModule0Topography::calcRaw(const std::vector<double>& rawValues) const
{
	if (rawValues.empty()) {
		return 0;
	}

	double min_value = DBL_MAX;
	for (double val : rawValues) {
		min_value = std::min(min_value, val);
	}

	return min_value;
}
